
#include <bur/plctypes.h>

#ifdef _DEFAULT_INCLUDES
	#include <AsDefault.h>
#endif
#include <fundef.h>


void _INIT ProgramInit(void)
{

	MC_Power_0.Axis					= &gAxisConveyor;

	MC_Home_0.Axis					= &gAxisConveyor;
	MC_Home_0.Position				= 0.0;
	MC_Home_0.HomingMode			= mcHOMING_DIRECT;
	MC_Home_0.BufferMode			= mcABORTING;

	MC_MoveAdditive_0.Axis			= &gAxisConveyor;
	MC_MoveAdditive_0.Distance		= 0.1;
	MC_MoveAdditive_0.Velocity		= 0.1;
	MC_MoveAdditive_0.Acceleration	= 1.0;
	MC_MoveAdditive_0.Deceleration	= 1.0;
	MC_MoveAdditive_0.BufferMode	= mcABORTING;

	MC_MoveVelocity_0.Axis			= &gAxisConveyor;
	MC_MoveVelocity_0.Velocity		= 0.1;
	MC_MoveVelocity_0.Acceleration	= 1.0;
	MC_MoveVelocity_0.Deceleration	= 1.0;
	MC_MoveVelocity_0.Direction		= mcDIR_POSITIVE;
	MC_MoveVelocity_0.BufferMode	= mcABORTING;

	MC_BR_MoveCyclicVelocity_0.Axis					= &gAxisConveyor;
	MC_BR_MoveCyclicVelocity_0.CyclicVelocity		= 0.1;
	MC_BR_MoveCyclicVelocity_0.InterpolationMode	= mcIPLM_LINEAR;
	MC_BR_MoveCyclicVelocity_0.AdvancedParameters.Acceleration			= 1.0;
	MC_BR_MoveCyclicVelocity_0.AdvancedParameters.Deceleration			= 1.0;
	MC_BR_MoveCyclicVelocity_0.AdvancedParameters.DisableJoltLimitation	= 1;

	MC_Stop_0.Axis					= &gAxisConveyor;
	MC_Stop_0.Deceleration			= 1.0;

	MC_ReadActualPosition_0.Axis	= &gAxisConveyor;
	MC_ReadActualPosition_0.Enable	= 1;

	MC_ReadActualVelocity_0.Axis	= &gAxisConveyor;
	MC_ReadActualVelocity_0.Enable	= 1;

	MC_ReadAxisInfo_0.Axis		= &gAxisConveyor;
	MC_ReadAxisInfo_0.Enable	= 1;

	MC_ReadAxisError_0.Axis		= &gAxisConveyor;
	MC_ReadAxisError_0.Enable	= 1;

	MC_ReadStatus_0.Axis	= &gAxisConveyor;
	MC_ReadStatus_0.Enable	= 1;

	ParIDInit.Num = 0;
	// 8AC122.60-2
	par_wrUI1(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_ENCOD_TYPE,				&MotorPar.ENCOD_TYPE);					MotorPar.ENCOD_TYPE					=5;	//ncRESOLVER;
	par_wrUI4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_SCALE_ENCOD_INCR,			&MotorPar.SCALE_ENCOD_INCR);			MotorPar.SCALE_ENCOD_INCR			=4096;
	par_wrUI1(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_ENCOD_POLEPAIRS,			&MotorPar.ENCOD_POLEPAIRS);				MotorPar.ENCOD_POLEPAIRS			=1;
	par_wrUI2(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_ENCOD_LINE_CHK_IGNORE,		&MotorPar.ENCOD_LINE_CHK_IGNORE);		MotorPar.ENCOD_LINE_CHK_IGNORE		=0;
	// 8MSA2M.R0-43
	par_wrUI2(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_TYPE,				&MotorPar.MOTOR_TYPE);					MotorPar.MOTOR_TYPE					=0x0002;
	par_wrUI2(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_COMPATIBILITY,		&MotorPar.MOTOR_COMPATIBILITY);			MotorPar.MOTOR_COMPATIBILITY		=0x0201;
	par_wrUI1(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_WIND_CONNECT,		&MotorPar.MOTOR_WIND_CONNECT);			MotorPar.MOTOR_WIND_CONNECT			=1;
	par_wrUI2(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_POLEPAIRS,			&MotorPar.MOTOR_POLEPAIRS);				MotorPar.MOTOR_POLEPAIRS			=3;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_BRAKE_CURR_RATED,	&MotorPar.MOTOR_BRAKE_CURR_RATED);		MotorPar.MOTOR_BRAKE_CURR_RATED		=0;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_BRAKE_TORQ_RATED,	&MotorPar.MOTOR_BRAKE_TORQ_RATED);		MotorPar.MOTOR_BRAKE_TORQ_RATED		=0;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_BRAKE_ON_TIME,		&MotorPar.MOTOR_BRAKE_ON_TIME);			MotorPar.MOTOR_BRAKE_ON_TIME		=0;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_BRAKE_OFF_TIME,		&MotorPar.MOTOR_BRAKE_OFF_TIME);		MotorPar.MOTOR_BRAKE_OFF_TIME		=0;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_TEMPSENS_PAR1,		&MotorPar.MOTOR_TEMPSENS_PAR1);			MotorPar.MOTOR_TEMPSENS_PAR1		=632;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_TEMPSENS_PAR2,		&MotorPar.MOTOR_TEMPSENS_PAR2);			MotorPar.MOTOR_TEMPSENS_PAR2		=2225;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_TEMPSENS_PAR3,		&MotorPar.MOTOR_TEMPSENS_PAR3);			MotorPar.MOTOR_TEMPSENS_PAR3		=-30;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_TEMPSENS_PAR4,		&MotorPar.MOTOR_TEMPSENS_PAR4);			MotorPar.MOTOR_TEMPSENS_PAR4		=5.7266;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_TEMPSENS_PAR5,		&MotorPar.MOTOR_TEMPSENS_PAR5);			MotorPar.MOTOR_TEMPSENS_PAR5		=36.0938;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_TEMPSENS_PAR6,		&MotorPar.MOTOR_TEMPSENS_PAR6);			MotorPar.MOTOR_TEMPSENS_PAR6		=62.9375;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_TEMPSENS_PAR7,		&MotorPar.MOTOR_TEMPSENS_PAR7);			MotorPar.MOTOR_TEMPSENS_PAR7		=87.25;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_TEMPSENS_PAR8,		&MotorPar.MOTOR_TEMPSENS_PAR8);			MotorPar.MOTOR_TEMPSENS_PAR8		=109.6016;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_TEMPSENS_PAR9,		&MotorPar.MOTOR_TEMPSENS_PAR9);			MotorPar.MOTOR_TEMPSENS_PAR9		=130.3984;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_TEMPSENS_PAR10,		&MotorPar.MOTOR_TEMPSENS_PAR10);		MotorPar.MOTOR_TEMPSENS_PAR10		=150;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_VOLTAGE_RATED,		&MotorPar.MOTOR_VOLTAGE_RATED);			MotorPar.MOTOR_VOLTAGE_RATED		=330;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_VOLTAGE_CONST,		&MotorPar.MOTOR_VOLTAGE_CONST);			MotorPar.MOTOR_VOLTAGE_CONST		=36;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_SPEED_RATED,			&MotorPar.MOTOR_SPEED_RATED);			MotorPar.MOTOR_SPEED_RATED			=6000;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_SPEED_MAX,			&MotorPar.MOTOR_SPEED_MAX);				MotorPar.MOTOR_SPEED_MAX			=12000;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_TORQ_STALL,			&MotorPar.MOTOR_TORQ_STALL);			MotorPar.MOTOR_TORQ_STALL			=0.4;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_TORQ_RATED,			&MotorPar.MOTOR_TORQ_RATED);			MotorPar.MOTOR_TORQ_RATED			=0.35;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_TORQ_MAX,			&MotorPar.MOTOR_TORQ_MAX);				MotorPar.MOTOR_TORQ_MAX				=1.6;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_TORQ_CONST,			&MotorPar.MOTOR_TORQ_CONST);			MotorPar.MOTOR_TORQ_CONST			=0.595;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_CURR_STALL,			&MotorPar.MOTOR_CURR_STALL);			MotorPar.MOTOR_CURR_STALL			=0.67;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_CURR_RATED,			&MotorPar.MOTOR_CURR_RATED);			MotorPar.MOTOR_CURR_RATED			=0.62;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_CURR_MAX,			&MotorPar.MOTOR_CURR_MAX);				MotorPar.MOTOR_CURR_MAX				=2.9;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_WIND_CROSS_SECT,		&MotorPar.MOTOR_WIND_CROSS_SECT);		MotorPar.MOTOR_WIND_CROSS_SECT		=0.044;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_STATOR_RESISTANCE,	&MotorPar.MOTOR_STATOR_RESISTANCE);		MotorPar.MOTOR_STATOR_RESISTANCE	=50;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_STATOR_INDUCTANCE,	&MotorPar.MOTOR_STATOR_INDUCTANCE);		MotorPar.MOTOR_STATOR_INDUCTANCE	=0.04;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_INERTIA,				&MotorPar.MOTOR_INERTIA);				MotorPar.MOTOR_INERTIA				=0.000008;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_COMMUT_OFFSET,		&MotorPar.MOTOR_COMMUT_OFFSET);			MotorPar.MOTOR_COMMUT_OFFSET		=0;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_TAU_THERM,			&MotorPar.MOTOR_TAU_THERM);				MotorPar.MOTOR_TAU_THERM			=900;
	par_wrR4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_MOTOR_WIND_TEMP_MAX,		&MotorPar.MOTOR_WIND_TEMP_MAX);			MotorPar.MOTOR_WIND_TEMP_MAX		=110;
	// load scaling
//	par_wrUI4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_SCALE_LOAD_UNITS,			&MotorPar.SCALE_LOAD_UNITS);			MotorPar.SCALE_LOAD_UNITS			=175929;
//	par_wrUI4(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_SCALE_LOAD_MOTOR_REV,		&MotorPar.SCALE_LOAD_MOTOR_REV);		MotorPar.SCALE_LOAD_MOTOR_REV		=50;
//	par_wrUI1(	(McAcpAxProcessParIDType*)&ParIDInit.List,	&ParIDInit.Num,	mcACPPAR_VCTRL_ENCOD_COUNT_DIR,		&MotorPar.VCTRL_ENCOD_COUNT_DIR);		MotorPar.VCTRL_ENCOD_COUNT_DIR		=0;
	
	MC_BR_ProcessParID_AcpAx_0.Axis				= &gAxisConveyor;
	MC_BR_ProcessParID_AcpAx_0.Execute			= 1;
	MC_BR_ProcessParID_AcpAx_0.DataAddress		= (UDINT)&ParIDInit.List;
	MC_BR_ProcessParID_AcpAx_0.NumberOfParIDs	= ParIDInit.Num;
	MC_BR_ProcessParID_AcpAx_0.Mode				= mcACPAX_PARID_SET;

	
	CyclicSetPositionEnable = 1;
	OffsetAddShuttleSector2 = 0.052+0.001;
	
	ConveyorPower = 0;
	ConveyorVelocity = 0.2;
	
}






void _CYCLIC ProgramCyclic(void)
{

	if(ConveyorPower)
	{
		MC_Power_0.Enable = 1;
		if(MC_Power_0.Status)
		{
			MC_Home_0.Execute = 1;
		}
		if(MC_Home_0.Done)
		{
			MC_BR_MoveCyclicVelocity_0.Enable = 1;
		}
	}
	else
	{
		MC_Power_0.Enable = 0;
		MC_Home_0.Execute = 0;
		MC_BR_MoveCyclicVelocity_0.Enable = 0;
	}
	if(MC_BR_MoveCyclicVelocity_0.Valid == 0)
		MC_BR_MoveCyclicVelocity_0.CyclicVelocity = 0;
	else
		MC_BR_MoveCyclicVelocity_0.CyclicVelocity = ConveyorVelocity;

	MC_Power(&MC_Power_0);
	MC_Home(&MC_Home_0);
	MC_MoveAdditive(&MC_MoveAdditive_0);
	MC_MoveVelocity(&MC_MoveVelocity_0);
	MC_BR_MoveCyclicVelocity(&MC_BR_MoveCyclicVelocity_0);
	MC_Stop(&MC_Stop_0);
	MC_ReadActualPosition(&MC_ReadActualPosition_0);
	MC_ReadActualVelocity(&MC_ReadActualVelocity_0);
	MC_ReadAxisInfo(&MC_ReadAxisInfo_0);
	MC_ReadAxisError(&MC_ReadAxisError_0);
	MC_ReadStatus(&MC_ReadStatus_0);
	MC_BR_ProcessParID_AcpAx(&MC_BR_ProcessParID_AcpAx_0);


	TOF_ConveyorSwitch.IN = gDiConveyorSwitch;
	TOF_ConveyorSwitch.PT = 100;
	TOF(&TOF_ConveyorSwitch);
	gDiConveyorSwitchFilter = TOF_ConveyorSwitch.Q;
	
	// add shuttle at trigger position
	if((gDiConveyorSwitchFilter) && (EdgeConveyorSwitchFilter == 0))
	{
		// search for first unused shuttle index
		for(i=1;i<=MAX_NUM_SHUTTLES;i++)
		{
			pShuttleAxis = (McAxisType*)gAssembly.Shuttle[i].Ref;
			if(pShuttleAxis->controlif == 0)
			{
				MasterRequested[i] = 1;
				MasterStartOffset[i] = MC_ReadActualPosition_0.Position;
				gAssembly.Shuttle[i].CyclicSetPositionOffset = OffsetAddShuttleSector2;
				gAssembly.Shuttle[i].CyclicSetRequest = 99;
				gAssembly.Shuttle[i].CyclicSetPosition = MC_ReadActualPosition_0.Position - MasterStartOffset[i] + gAssembly.Shuttle[i].CyclicSetPositionOffset;
				gAssembly.Shuttle[i].CyclicSetVelocity = MC_ReadActualVelocity_0.Velocity;
				AddShuttleAxis = i;
				break;
			}
		}
	}
	EdgeConveyorSwitchFilter = gDiConveyorSwitchFilter;

	switch(AddShuttleStep)
	{
		case 0:
			if(AddShuttleAxis)
			{
				MC_BR_SecAddShuttle_0.Execute	= 1;
				MC_BR_SecAddShuttle_0.Sector	= (McSectorType*)gAssembly.Sector[2].Ref;
				MC_BR_SecAddShuttle_0.Position	= gAssembly.Shuttle[AddShuttleAxis].CyclicSetPosition;
				MC_BR_SecAddShuttle_0.Velocity	= gAssembly.Shuttle[AddShuttleAxis].CyclicSetVelocity;
				AddShuttleStep++;
			}
		break;
		case 1:
			if(MC_BR_SecAddShuttle_0.Done)
			{
				pShuttleAxis = (McAxisType*)gAssembly.Shuttle[AddShuttleAxis].Ref;
				*pShuttleAxis = MC_BR_SecAddShuttle_0.Axis;
				AddShuttleStep = 10;
			}
			else if(MC_BR_SecAddShuttle_0.Busy == 0)
			{
				AddShuttleAxis = 0;
				AddShuttleStep = 0;
			}
		break;
		
		case 10:
			MC_BR_MoveCyclicElasticPos_0[AddShuttleAxis].Axis				= (McAxisType*)gAssembly.Shuttle[AddShuttleAxis].Ref;
			MC_BR_MoveCyclicElasticPos_0[AddShuttleAxis].Enable				= 1;
			MC_BR_MoveCyclicElasticPos_0[AddShuttleAxis].InterpolationMode	= mcIPLM_OFF;
			MC_BR_MoveCyclicElasticPos_0[AddShuttleAxis].AdvancedParameters.Velocity				= 5.0;
			MC_BR_MoveCyclicElasticPos_0[AddShuttleAxis].AdvancedParameters.Acceleration			= 50.0;
			MC_BR_MoveCyclicElasticPos_0[AddShuttleAxis].AdvancedParameters.Deceleration			= 50.0;
			MC_BR_MoveCyclicElasticPos_0[AddShuttleAxis].AdvancedParameters.Jerk					= 0.0;
			MC_BR_MoveCyclicElasticPos_0[AddShuttleAxis].AdvancedParameters.DisableJoltLimitation	= 1;
			AddShuttleStep++;
		break;
		case 11:
			if(MC_BR_MoveCyclicElasticPos_0[AddShuttleAxis].Valid)
			{
				AddShuttleAxis = 0;
				AddShuttleStep = 0;
			}
		break;
	}	
	
	
	if(CyclicSetPositionEnable)
	{
		for(i=1;i<=MAX_NUM_SHUTTLES;i++)
		{
			if(gAssembly.Shuttle[i].CyclicSetRequest == 0)
			{
				MasterRequested[i] = 0;
			}
			if(gAssembly.Shuttle[i].Valid)
			{
				// check for CyclicPosition request
				if(gAssembly.Shuttle[i].CyclicSetRequest == 3)
				{
					MasterRequested[i] = 1;
					MasterStartOffset[i] = MC_ReadActualPosition_0.Position;
					gAssembly.Shuttle[i].CyclicSetRelease = 1;
				}
			}	
			// cyclic copy assigned master-setposition to shuttle
			if(MasterRequested[i])
			{
				gAssembly.Shuttle[i].CyclicSetPosition = MC_ReadActualPosition_0.Position - MasterStartOffset[i] + gAssembly.Shuttle[i].CyclicSetPositionOffset;
				gAssembly.Shuttle[i].CyclicSetVelocity = MC_ReadActualVelocity_0.Velocity;
			}
		}
	}

	MC_BR_SecAddShuttle_AcpTrak(&MC_BR_SecAddShuttle_0);
	MC_BR_SecAddShuttle_0.Execute = 0;
		
	for(i=1;i<=MAX_NUM_SHUTTLES;i++)
	{
		MC_BR_MoveCyclicElasticPos_0[i].CyclicPosition = gAssembly.Shuttle[i].CyclicSetPosition;
		MC_BR_ElasticMoveCycPos_AcpTrak(&MC_BR_MoveCyclicElasticPos_0[i]);
		if(((MC_BR_MoveCyclicElasticPos_0[i].Busy == 0) || (MC_BR_MoveCyclicElasticPos_0[i].CommandAborted) || (MC_BR_MoveCyclicElasticPos_0[i].Error)) && (MC_BR_MoveCyclicElasticPos_0[i].Enable))
		{
			MC_BR_MoveCyclicElasticPos_0[i].Enable	= 0;
			gAssembly.Shuttle[i].CyclicSetRequest = 0;
		}
	}
	

}






void _EXIT ProgramExit(void)
{

}

